#!/usr/bin/env python3
"""
@file sensor.launch.py
@brief RobotOne传感器节点启动文件
@author AI进化论-花生
@date 2024
@version 0.0.0
@license MIT

本文件提供了RobotOne传感器节点的启动配置，支持参数配置和节点管理。
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node
from launch.conditions import IfCondition


def generate_launch_description():
    """生成启动描述"""
    
    # 声明启动参数
    publish_rate_arg = DeclareLaunchArgument(
        'publish_rate',
        default_value='1.0',
        description='传感器数据发布频率 (Hz)'
    )
    
    queue_size_arg = DeclareLaunchArgument(
        'queue_size',
        default_value='10',
        description='消息队列大小'
    )
    
    frame_id_arg = DeclareLaunchArgument(
        'frame_id',
        default_value='imu_link',
        description='坐标系ID'
    )
    
    enable_logging_arg = DeclareLaunchArgument(
        'enable_logging',
        default_value='true',
        description='是否启用详细日志'
    )
    
    # 传感器节点
    sensor_node = Node(
        package='robotone',
        executable='sensor_node',
        name='sensor_node',
        output='screen',
        parameters=[{
            'publish_rate': LaunchConfiguration('publish_rate'),
            'queue_size': LaunchConfiguration('queue_size'),
            'frame_id': LaunchConfiguration('frame_id'),
        }],
        remappings=[
            # 可以在这里添加主题重映射
            # ('sensor_data', 'imu/data'),
        ]
    )
    
    # 启动信息日志
    startup_info = LogInfo(
        msg=[
            'Starting RobotOne sensor node with parameters:',
            '  - Publish rate: ', LaunchConfiguration('publish_rate'), ' Hz',
            '  - Queue size: ', LaunchConfiguration('queue_size'),
            '  - Frame ID: ', LaunchConfiguration('frame_id'),
            '  - Logging enabled: ', LaunchConfiguration('enable_logging')
        ],
        condition=IfCondition(LaunchConfiguration('enable_logging'))
    )
    
    return LaunchDescription([
        publish_rate_arg,
        queue_size_arg,
        frame_id_arg,
        enable_logging_arg,
        startup_info,
        sensor_node,
    ])
